#include "vsPhotometric.h"


extern RosConnector *connector;

VS_photometric::VS_photometric(cv::Mat desImg_, float focalLengthPIX_, float u0_, float v0_) {
    bord1=(desImg_.rows-512)/2;
	bord2=(desImg_.cols-512)/2;
	focalLengthPIX = focalLengthPIX_;
	u0 = u0_;
	v0 = v0_;
	cv::Mat desImg = desImg_.clone();
	if (desImg.channels() == 3)
	{
		cv::cvtColor(desImg, desImgGray, cv::COLOR_BGR2GRAY);
	}
	else
	{
		desImgGray = desImg.clone();
	}
	//desImgGray = desImgGray / 4;
	pixelNum = (desImg.rows - 2 * bord1) * (desImg.cols - 2 * bord2);

	pixInfo = new Luminance[pixelNum];
	Hsd = Eigen::MatrixXd::Zero(6, 6);
	diagHsd = Eigen::MatrixXd::Zero(6, 6);
}
VS_photometric::~VS_photometric()
{
	if (pixInfo != NULL)
	{
		std::cout << pixInfo[0].x << std::endl;
		delete[] pixInfo;
		pixInfo = NULL;
		std::cout << "pixInfo已删除" << std::endl;
	}
}

void VS_photometric::getInteraction() {
	Lsd = Eigen::MatrixXd::Zero(pixelNum, 6);
	for (unsigned int m = 0; m < pixelNum; m++)
	{
		double Zinv = 1 / ZInCam;
		double x = pixInfo[m].x;
		double y = pixInfo[m].y;
		double Ix = pixInfo[m].Ix;
		double Iy = pixInfo[m].Iy;
		// Lsd = gradient * Lx
		Lsd(m, 0) = Ix * Zinv;
		Lsd(m, 1) = Iy * Zinv;
		Lsd(m, 2) = -(x * Ix + y * Iy) * Zinv;
		Lsd(m, 3) = -Ix * x * y - (1 + y * y) * Iy;
		Lsd(m, 4) = (1 + x * x) * Ix + Iy * x * y;
		Lsd(m, 5) = Iy * x - Ix * y;

	}
	Hsd = Lsd.transpose() * Lsd;
	//std::cout << Hsd(0, 0) << std::endl;
	for (unsigned int i = 0; i < 6; i++)
		diagHsd(i, i) = Hsd(i, i);
}
void VS_photometric::servoing(cv::Mat camGetImage) {
	cv::Mat cameraGetGray;
	if (camGetImage.channels() == 3)
	{
		cv::cvtColor(camGetImage, cameraGetGray, cv::COLOR_BGR2GRAY);
	}
	else
	{
		cameraGetGray = camGetImage.clone();
	}
	int l = 0;
	Eigen::MatrixXd errorLuminance;
	errorLuminance = Eigen::MatrixXd::Zero(pixelNum, 1);
	float Ix, Iy;
	for (unsigned int i = bord1; i < cameraGetGray.rows - bord1; i++)
	{
		uchar * camGetImageRow = cameraGetGray.ptr<uchar>(i);
		uchar * desImageRow = desImgGray.ptr<uchar>(i);
		for (unsigned int j = bord2; j < cameraGetGray.cols - bord2; j++)
		{
			errorLuminance(l, 0) = camGetImageRow[j] - desImageRow[j];

			double x = 0, y = 0;
			convertPoint(j, i, x, y);
			pixInfo[l].x = x;
			pixInfo[l].y = y;
			pixInfo[l].Ix = derivativeFilterX(cameraGetGray, i, j)* focalLengthPIX;
			pixInfo[l].Iy = derivativeFilterY(cameraGetGray, i, j)* focalLengthPIX;
			l++;
		}
	}
	getInteraction();
	double lambda, mu;
	float averageError = errorLuminance.squaredNorm() / pixelNum;
	std::cout << "error:" << averageError << std::endl;
	allErrorMean.push_back(averageError);
	if (averageError >= 3000)  // real 500
	{
		lambda = 30; mu = 0.02;  // real 20      //30
	}
	else if (averageError >= 10 && averageError < 3000)  // real 500
	{
		lambda = 3; mu = 0.0001;        //3
	}
	else
	{
		lambda = 0.3; mu = 0.0001;
	}
	/*typedef Eigen::Matrix<double, 6, 6> Matrix6d;
	Eigen::EigenSolver<Matrix6d> eigen_solver(Hsd);
	std::cout << "特征值为：" << eigen_solver.eigenvalues() << std::endl;*/
	cameraVel =( -lambda * ((Hsd + mu * diagHsd).inverse())*Lsd.transpose() * errorLuminance).cast<float>();
	//cameraVel =( -lambda * ((diagHsd).inverse())*Lsd.transpose() * errorLuminance).cast<float>();
	//cameraVel = (-lambda * ((Hsd).inverse())*Lsd.transpose() * errorLuminance).cast<float>();
	norm_cameraVel = Hsd.inverse()*Lsd.transpose()*errorLuminance;
	Eigen::MatrixXd reproj_error;
	reproj_error = Eigen::MatrixXd::Zero(pixelNum, 1);
	reproj_error = Lsd * norm_cameraVel;
	//std::cout << "重投影误差为" << (errorLuminance - reproj_error).squaredNorm() << std::endl;
	//std::cout << "重投影误差2为" << (errorLuminance - 2 * reproj_error).squaredNorm() << std::endl;
	allCameraVel.push_back(cameraVel);
}
void VS_photometric::showerror(cv::Mat camGetImage)
{
	cv::Mat cameraGetGray;
	if (camGetImage.channels() == 3)
	{
		cv::cvtColor(camGetImage, cameraGetGray, cv::COLOR_BGR2GRAY);
	}
	else
	{
		cameraGetGray = camGetImage;
	}
	cv::Mat errcutimg;
	cv::Mat error_Mat = (cameraGetGray - desImgGray + 255) / 2;
	cv::Rect rect(bord2, bord1, 512, 512);
	errcutimg = error_Mat(rect);

	imshow("error", errcutimg);
	// cv::waitKey(1);
}
void VS_photometric::VScontrol()
{

	//打开摄像头
	cv::VideoCapture video;
	video.open("path");//读取视频文件
	video.open(0);//读取摄像头
	if (!video.isOpened())
	{
		std::cout << "摄像头未打开！" << std::endl;
	}

	// ros::spinOnce();
    for(int i=0; i<6; i++){
        // connector->joint_simulation_position_rec[i] = connector->joint_simulation_position[i];
        connector->joint_simulation_position_rec[i] = connector->motors[i];
    }

	for (int i = 0; i < 20000; i++)      /////////////////伺服
	{
		ros::spinOnce();
		std::cout << "servoing period：" << i << std::endl;
		// 获取相机信息


		cv::Mat cameraGet;
		video >> cameraGet;
		if (cameraGet.empty())
		{
			std::cout << "没有获取图像" << std::endl;
			break;
		}

		cv::cvtColor(cameraGet, cameraGet, cv::COLOR_BGR2GRAY);
		// 伺服控制
		servoing(cameraGet);                          //速度控制
		showerror(cameraGet);                         //////可以注释掉，可以在窗口显示

                                                        //////////把vslaw.cameraVel传给机器人
		//调用到相机的雅克比
        vector<double> motor_angles_in=connector->present_position_get();//返回电机角
        connector->kine.Param_joints_update(motor_angles_in);//根据电机角更新关节角
       

		MatrixXd cameraVel_base(6,1);//相机速度在基坐标系下的表示
        MatrixXd cameraVel_temp;
		cameraVel_temp<<cameraVel(0,0),cameraVel(1,0),cameraVel(2,0),cameraVel(3,0),cameraVel(4,0),cameraVel(5,0);//转一下数据格式
		// cameraVel_temp<<10,0,0,0,0,0;
		cameraVel_base=connector->kine.Get_Vel_Translation()*cameraVel_temp;       

		MatrixXd joints_vel(6,1); //关节速度
		joints_vel=connector->kine.Jacobian_CamToBase()*cameraVel_base;
		vector<double> joints_vel_vector(6);
		for(int i=0;i<6;i++)  //转数据格式
		{
			joints_vel_vector[i]=joints_vel(i,0);
		}

		MatrixXd motors_vel(8,1); //电机速度
		motors_vel=connector->kine.vel_maping_joints2motors()*joints_vel;
        vector<double> motors_vel_vector(10);
		// for(int i=0;i<8;i++)
		// {
		// 	motors_vel_vector[i]=motors_vel(i,0);
		// }
		motors_vel_vector[0] = motors_vel(0,0);//转换数组格式
		motors_vel_vector[1] = 0;
		motors_vel_vector[2] = 0;
		motors_vel_vector[3] = motors_vel(5,0);
		motors_vel_vector[4] = motors_vel(6,0);
		motors_vel_vector[5] = motors_vel(7,0);
		motors_vel_vector[6] = motors_vel(1,0);
		motors_vel_vector[7] = motors_vel(2,0);
		motors_vel_vector[8] = motors_vel(3,0);
		motors_vel_vector[9] = motors_vel(4,0);
		//控制机器人运动
		connector->velocity_mode();//初始化速度模式
		connector->pub_all_robot(motors_vel_vector);//输入8个电机速度 单位：rad/s
		
	}
}

void VS_photometric::VScontrol_simulation()
{

	std::cout<<"-----------------------FLAG 开始VScontrol_simulation----------------------"<<endl;

    // ros::spinOnce();
    for(int i=0; i<6; i++){
        // connector->joint_simulation_position_rec[i] = connector->joint_simulation_position[i];
        connector->joint_simulation_position_rec[i] = connector->motors[i];
    }

	for (int loop = 0; loop < 10; loop++)      /////////////////伺服   loop设置为20000
	{
		ros::spinOnce();
		cout<<"-----------------------------Fservoing period："<<loop<<"------------------------"<<endl;
		// 获取相机信息
        
		uint8_t image_uint8[786432]; //512*512*3 根据相机像素尺寸进行修改；
		for (int i=0;i<connector->image_data.size();i++)//转数据格式
		{
			image_uint8[i]=connector->image_data[i];
		}

		cv::Mat cameraGet(connector->image_height,connector->image_width,CV_8UC3,image_uint8);
		if (cameraGet.empty())
		{
			std::cout << "没有获取图像" << std::endl;
			continue;
		}
        cv::flip(cameraGet,cameraGet,0);
		cv::cvtColor(cameraGet, cameraGet, cv::COLOR_BGR2GRAY);

		cv::imshow("cameraGet",cameraGet);
		//cv::waitKey(1);

		// 伺服控制
	    
		servoing(cameraGet);                          //速度控制
		showerror(cameraGet);                         //////可以注释掉，可以在窗口显示
		//cv::waitKey(1);
        //////////把vslaw.cameraVel传给机器人
        // cout<<"vslaw.cameraVel"<<cameraVel<<endl;

        connector->kine.Param_joints_update_simulation(connector->joint_simulation_position);//更新仿真中的kine对象里的关节角
		MatrixXd cameraVel_base(6,1);//相机速度在基坐标系下的表示
        MatrixXd cameraVel_temp(6,1);
		cameraVel_temp<<cameraVel(0,0),cameraVel(1,0),cameraVel(2,0),cameraVel(3,0),cameraVel(4,0),cameraVel(5,0);//转一下数据格式
		
		// cameraVel_temp<<0,0,0,0,0,50;//x轴反

		cameraVel_base=connector->kine.Get_Vel_Translation()*cameraVel_temp;  //   ////////////////////////符号  
        // cout<<"cameraVel_temp="<<cameraVel_temp<<endl;
		// cout<<"cameraVel_base="<<endl<< cameraVel_base<<endl;

		MatrixXd joints_vel(6,1); //关节速度
		joints_vel=connector->kine.Jacobian_CamToBase()*cameraVel_base;
        // cout<<"关节速度："<<joints_vel<<endl;
		

        // cout<<"joints_vel="<<joints_vel<<endl;

		vector<double> joints_vel_vector(6);
		// for(int i=0;i<6;i++)  //转数据格式
		// {
		// 	joints_vel_vector[i]=joints_vel(i,0);
		// }
		// connector->pub_total_velocity(joints_vel_vector); //仿真中控制关节速度
		
		for(int i=0;i<6;i++)  //转数据格式
		{
			// joints_vel_vector[i]=joints_vel(i,0)*0.0000000000000000000001 + connector->joint_simulation_position[i];
			// joints_vel_vector[i]=connector->joint_simulation_position[i];
			connector->joint_simulation_position_rec[i]=joints_vel(i,0)*0.05 + connector->joint_simulation_position_rec[i];
			// joints_vel_vector[i] = connector->joint_simulation_position_rec[i];
			connector->motors[i] = connector->joint_simulation_position_rec[i];
		}
		if (connector->i_log == 4 & connector->i_screw == 1){
			connector->motors[10] = 0;			
		}else if(connector->i_log == 2){
			connector->motors[10] = 238 - 49.07 / tan(pi / 2 - connector->motors[0]);
		}else if(connector->i_log == 3){
			connector->motors[10] = 238 - 55.41 / tan(pi / 2 - connector->motors[0]);
		}else{
			connector->motors[10] = 238 - 50.59 / tan(pi / 2 - connector->motors[0]);
		}
		connector->pub_total_position(connector->motors,connector->i_log); //仿真中控制关节速度
	}

	cout<<"------------------------退出视觉伺服程序-----------------"<<endl;
}

// 求矩阵的违逆
MatXf pinv(MatXf x)
{
	Eigen::JacobiSVD<MatXf> svd(x, Eigen::ComputeFullU | Eigen::ComputeFullV);
	float  pinvtoler = 1.e-8;
	MatXf singularValues_inv = svd.singularValues();
	for (long i = 0; i < x.cols(); ++i) {
		if (singularValues_inv(i) > pinvtoler)
			singularValues_inv(i) = 1.0 / singularValues_inv(i);
		else singularValues_inv(i) = 0;
	}
	return svd.matrixV()*singularValues_inv.asDiagonal()*svd.matrixU().transpose();
}